#include <ros/ros.h>
#include <ros/console.h>
#include <vector>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/TransformStamped.h"
#include "nav_msgs/Odometry.h"
#include "std_srvs/Empty.h"

#include <tf2/LinearMath/Quaternion.h>
#include "tf2/utils.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"

using namespace std;
class AutoChartgeControl
{
  public:
    AutoChartgeControl(ros::NodeHandle);
    ~AutoChartgeControl();

    void pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &pose_msg);
    void odom_callback(const nav_msgs::Odometry &odom_msg);
    void publishTf();
    bool moveToPark(geometry_msgs::PoseStamped charger_goal_pose_odom, double Kp_rho, double Kp_alpha, double Kp_beta, double max_vel, double max_angular, double stop_dis);
    bool rotateToPark(double max_yaw_diff, double max_angular);
    void spinOnce();

  private:

    ros::Publisher vel_pub_;
    ros::Subscriber odom_subscriber_;
    ros::Subscriber pose_subscriber_;
    nav_msgs::Odometry current_odom_;
    geometry_msgs::Transform tf_goal_tansform;
    geometry_msgs::PoseStamped charger_goal_pose_odom;

    tf2_ros::Buffer tfbuffer_;
    tf2_ros::TransformBroadcaster broadcaster_;
    std::unique_ptr<tf2_ros::TransformListener> listener;
    ros::Timer timer_;

    int check_times_;
};
